import time
import sensor
import math
import image
import ustruct
from pyb import UART
import lcd

uart = UART(2, 115200, timeout_char=200)
uart.init(115200, bits=8, parity=None, stop=1)  # init with given parameters

threshold_index = 00000000000000000                           # 0 for red, 1 for green, 2 for blue
thresholds = [
      (93, 5, 12, 127, -56, 82),
    #(30, 100, 15, 127, 15, 127),  # generic_red_thresholds
    (30, 100, -64, -8, -32, 32),  # generic_green_thresholds
    (0, 30, 0, 64, -128, 0),      # generic_blue_thresholds
    (82, 100, 75, -49, -22, 31),  # generic_white_thresholds
    (21, 83, 32, 65, 31, 63),
]
lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)# QVGA的中心坐标：160,120
sensor.skip_frames(time=2000)    # 跳过2000毫秒的帧让相机图像在改变相机设置后稳定下来
sensor.set_auto_gain(False)      # 必须关闭才能进行颜色跟踪
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_auto_whitebal(False)  # 必须关闭才能进行颜色跟踪
clock = time.clock()

def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob = blob
            max_size = blob.pixels()
    return max_blob


def find_max_blob(blobs):
    max_size = 0
    largest_blob = None
    for blob in blobs:
        if blob[2] * blob[3] > max_size:
            max_size = blob[2] * blob[3]
            largest_blob = blob
    return largest_blob

def send_data(Target_x,Target_y,target_distance,img_w,img_h):
    global uart;
    FistByte = bytearray([0xb3,0xb3])     # 帧头
    uart.write(FistByte)                  # 写到串口
    uart.write(str(Target_x))
    uart.write(bytearray([0x20]))   # 发送空格
    uart.write(str(Target_y))
    uart.write(bytearray([0x20]))
  uart.write(str(target_distance))
    uart.write(bytearray([0x20]))
    uart.write(str(img_w))
    uart.write(bytearray([0x20]))
    uart.write(str(img_h))
    uart.write(bytearray([0x20]))
    EndByte = bytearray([0x0d,0x0a])     # 帧尾,换行和回车的ascll
    uart.write(EndByte)



while True:
    clock.tick()
    img = sensor.snapshot()#获取一张图片
    lcd.display(img)
    TargetBlobs = img.find_blobs([thresholds[threshold_index]])

    #如果找到了目标颜色
    if TargetBlobs:
        largest_blob = find_max_blob(TargetBlobs)
        center_x=largest_blob[5]
        center_y=largest_blob[6]
        W=largest_blob[2]
        H=largest_blob[3]

        # 这些值取决于max_blob不是圆形的，否则它们将不稳定.
        # 检查max_blob是否显著偏离圆形
        if largest_blob.elongation() > 0.5:
            img.draw_edges(largest_blob.min_corners(), color=(255, 0, 0))
            img.draw_line(largest_blob.major_axis_line(), color=(0, 255, 0))
            img.draw_line(largest_blob.minor_axis_line(), color=(0, 0, 255))

        # 这些值始终是稳定的。

       # img.draw_rectangle(160,120,35,35)
        img.draw_cross(center_x, center_y)

        # 注意-max_blob旋转仅限于0-180。
        img.draw_keypoints(
            [(center_x, center_y, int(math.degrees(largest_blob.rotation())))], size=20
        )
        # 计算距离
        focal_length = 2.80  # 焦距，单位：毫米
        real_width = 6.0  # 实际物体宽度，单位：厘米
        pixel_width = largest_blob.h()
        distance = ((real_width * focal_length) / pixel_width)*100

        print("Distance: %.2f cm\r\n" % distance)
        #img.draw_string(10,10,"Distance",color={255,0,0})
        send_data(center_x,center_y,distance,W,H)      # 发送数据
        print(center_x,center_y,distance,W,H)
        print(f"x={center_x} y={center_y}\r\n W={W} H={H}")
        img.draw_string(10, 10, "Distance:%.2f cm"%distance, size=60,color=(0, 255, 0))  # 在坐标(10, 10)处绘制红色文字"Hello OpenMV!"
        # img.draw_string(10,30,"distance:" ,color=(0,255,0))  # 在坐标(10, 30)处绘制绿色文字，显示温度值

        print(clock.fps())
